import logging
import time

import cv2

import settings as st
from control.task import Task, TASK_LOOP
from vision import tf_light
from vision.camera import IMG_WIDTH, IMG_HEIGHT


logger = logging.getLogger(__name__)



class TaskScanTF(Task):
    # scheme 1. 起始态为红灯, 按方向算出颜色: 当前算法下, ctr_x在红灯中央时, 红灯变为黄灯不会被认为是change
    # scheme 2. 检测到灯色变化时, 通过串口light change事件: 当 ctr_x在红灯和绿灯之间时, 红黄灯同时亮起会导致识别到的灯频繁变化,导致两灯同时亮起时触发大量light change事件(实际上只是两个灯同时亮起)
    # scheme 3. 根据规则, 黄灯亮后2~3秒即转为绿灯. 为解决有时频繁产生light change事件的问题, 考虑到change到绿灯之后便不再有change,遂延时3s发送change, 这期间若有新的change则忽略, 并重新开始计时
    #       缺陷: 与scheme 1 同, 只有一次变化
    #       解法: light change 则喂狗, 后续循环测时间, 仅超时后才发送
    # =================================================================================
    # 在场地适应后发现, 红绿灯的变化方式是, 初始状态为红色, 经机器人触发后, 红绿灯进入倒计时, 
    # 最后红灯灭, 绿灯亮.  几秒之后, 绿灯灭, 黄灯亮, 接着黄灯灭, 红灯亮.
    # 因此, 基于变化判断灯色变得简单:
    #   1. 初始为红灯, 变化之后唯一的可能是红变绿, 不必采取喂狗的方法了.
    #   2. 各灯色的位置已确定, 且同一时刻只亮一盏灯, 故可通过灯位置变化的方向计算颜色.
    def __init__(self, cap):
        Task.__init__(self, TASK_LOOP)
        self.cap = cap
        self._last_light = None
        self._last_changed_time = None
        self._light_changed = False  # light change发生过
        self._dog_time = 3.5  # s, light change 之后, 超过这个时间之后才发送
        # self._changed_time = 0
        self._last_excute_time = 0  # 上次执行work()的时间
        self._ovettime_reset = 10 # s, 若距离上次执行超过该时间, 则重置一些变量
        
        # self._red    = 0
        # self._yellow = 1
        # self._green  = 2
        # self._cur_color = 0  # 根据规则, 最开始时一定是红灯, 灯色变化顺序为Red-Yellow-green
        self._direction_to_green = None  # 1 or -1, direction of tf light change from red to green
        self._change_accum = 0
        
    def work2(self):
        ret, raw = self.cap.read()
        if not ret:
            logger.info('read camera failed')
            return '', []
        # elif __debug__:
            # cv2.imshow('raw', raw)
        
        light = tf_light.detect_traffic_light(raw, seg_y=IMG_HEIGHT*2/5,
            r_light_min=IMG_WIDTH/40, r_light_max=IMG_WIDTH/10)
        if light is None:
            return '', []
        elif self._last_light is None:
            self._last_light = light
            return '', []
        
        # 交通灯亮区被表示为图像中的一个圆, 圆心(x,y), 半径r
        (cx, cy), cr = light                # current x,y,r
        (lx, ly), lr = self._last_light     # last x,y,r
        delta_x = cx - lx
        self._last_light = light
        # print('light{}, llight{}, delta_x:{}'.format(light, self._last_light,delta_x))
        # 当检测到的亮区位置变化幅度超过其半径的时, 说明灯色发生了变化
        if abs(delta_x) > lr:
            self._light_changed = True
            self._feed_dog()
            logger.debug("traffic light change detected, let's feed the dog.")        
        if self.dog() and self._light_changed:
            self._light_changed = False  
            logger.debug("send LB_TF_CHANGED")

            return st.LB_TF_CHANGED, []
        return '', []
    
    def work(self):
        self._reset_when_overtime()
        
        ret, raw = self.cap.read()
        if not ret:
            logger.info('read camera failed')
            return '', []
        
        light = tf_light.detect_traffic_light(raw, seg_y=IMG_HEIGHT/3,
            r_light_min=IMG_WIDTH/40, r_light_max=IMG_WIDTH/10)
        if light is None:
            return '', []
        elif self._last_light is None:
            self._last_light = light
            return '', []
        
        # 交通灯亮区被表示为图像中的一个圆, 圆心(x,y), 半径r
        (cx, cy), cr = light                # current x,y,r
        (lx, ly), lr = self._last_light     # last x,y,r
        delta_x = cx - lx
        self._last_light = light
        # print('light{}, llight{}, delta_x:{}'.format(light, self._last_light,delta_x))
        # 当检测到的亮区位置变化幅度超过其半径的时, 说明灯色发生了变化
        if abs(delta_x) > lr:
            logger.debug("send LB_TF_CHANGED")
            return st.LB_TF_CHANGED, []
        return '', []
    
    # def get_dir(self, delta_x):
            # """get the direction"""
            # return 1 if delta_x > 0 else -1
    
    def _reset_when_overtime(self):
        """针对赛时两次机会之间, 不关闭树莓派, 一些变量仍处于上次的状态, 造成
        误输出, 因而需要将一些变量置为初始状态."""
        current_time = time.time()
        if (current_time - self._last_excute_time) > self._ovettime_reset:
            self._reset()
        self._last_excute_time = current_time
    
    def _reset(self):
        self._last_light = None
        # self._change_accum = 0
        logger.info('task tf reset.')
        
    # def _change_color(self, dir_):
        # """change the color of tf light according the direction of change."""
        # if self._direction_to_green is None:
            # return
        # 方向同则变为下一色, 否则变回上一色
        # if dir_ == self._direction_to_green:
            # self._cur_color += 1
        # else:
            # self._cur_color -= 1
    
    def dog(self):
        """灵感来自Watch Dog in MCU, 返回值为True则代表已超时, 可发送light change事件"""
        # if self._last_changed_time is None:
            # self._last_changed_time = time.time()
            # return False
        if self._last_changed_time is None:  # 初始状态, 红灯, 不发
            return False
        elif (time.time() - self._last_changed_time) > self._dog_time: # 超时, 发
            return True
        else:  # 时间间隔太短, 不发
            return False
    
    def _feed_dog(self):
        self._last_changed_time = time.time()



if __name__ == '__main__':
    logging.basicConfig(level=logging.DEBUG)
    cap = cv2.VideoCapture(r'../../img_and_videos/tf-light.avi')
    # cap.set(cv2.CAP_PROP_POS_MSEC, 2000)
    # cap = cv2.VideoCapture(0)
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, IMG_WIDTH)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, IMG_HEIGHT)
    
    tk = TaskScanTF(cap)
    
    while True:
        tk.work()
        
        if cv2.waitKey(20) == 27:  # Esc
            break
    
